#ifndef FINE_ALIGNMENT_FINE_ALIGNER_HPP_
#define FINE_ALIGNMENT_FINE_ALIGNER_HPP_

#include <rclcpp/rclcpp.hpp>
#include <rclcpp_lifecycle/lifecycle_node.hpp>
#include <rclcpp_lifecycle/lifecycle_publisher.hpp>
#include <lifecycle_msgs/msg/transition.hpp>
#include <lifecycle_msgs/msg/transition.hpp>
#include "fine_alignment/center_json.hpp"
#include "fine_alignment/vel_json.hpp"
#include <std_msgs/msg/string.hpp>
#include <std_msgs/msg/bool.hpp>
#include <geometry_msgs/msg/twist.hpp>
#include <jsoncpp/json/json.h>
#include <memory>
#include <vector>
#include <deque>
#include <cmath>
#include <chrono>

namespace fine_alignment {

    using CallbackReturn = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn;

    // 状态机枚举
    enum ControlState {
        IDLE,          // 空闲状态
        SAMPLING,      // 采样状态
        MOVING,        // 移动状态
        VERIFYING,     // 验证状态
        REACHED        // 已到达状态
    };

    struct SamplePoint {
        double x;
        double y;
        double z;
    };

    class FineAligner : public rclcpp_lifecycle::LifecycleNode {
    public:
        explicit FineAligner(const rclcpp::NodeOptions & options = rclcpp::NodeOptions());
        ~FineAligner() override = default;

        // 生命周期回调
        CallbackReturn on_configure(const rclcpp_lifecycle::State & state) override;
        CallbackReturn on_activate(const rclcpp_lifecycle::State & state) override;
        CallbackReturn on_deactivate(const rclcpp_lifecycle::State & state) override;
        CallbackReturn on_cleanup(const rclcpp_lifecycle::State & state) override;

    private:
        
        // 参数声明
        void declare_parameters();

        // 回调函数
        void centerJsonCallback(const std_msgs::msg::String::SharedPtr msg);
        void controlLoop();

        // 状态处理函数
        void handleIdleState();
        void handleSamplingState();
        void handleMovingState();
        void handleVerifyingState();
        void handleReachedState();

        // 核心功能
        void startSampling();
        void processSamples();
        void calculateGaussianWeightedAverage();
        void startMovingToTarget();
        void stopRobot();
        void publishVelocity();
        void publishReachedStatus(bool success);
        void checkSamplingTimeout();

        // 辅助函数
        double calculateMovementTime(double distance, double max_vel, double max_acc);
        void applyVelocitySmoothing(double& linear_x, double& linear_y, double& angular_z,
                                double target_linear_x, double target_linear_y, double target_angular_z);


        // 成员变量
        rclcpp::Subscription<std_msgs::msg::String>::SharedPtr center_json_sub_;
        std::shared_ptr<rclcpp_lifecycle::LifecyclePublisher<std_msgs::msg::String>> cmd_vel_pub_;
        std::shared_ptr<rclcpp_lifecycle::LifecyclePublisher<std_msgs::msg::Bool>> reached_pub_;
        rclcpp::TimerBase::SharedPtr control_timer_;

        // 参数名称
        std::string center_json_topic_;
        std::string cmd_vel_topic_;
        std::string reached_topic_;

        // json类实体
        CenterJsonConverter center_coords;
        VelocityJsonConverter vel_cmd;

        // 状态变量
        ControlState control_state_;
        bool state_is_active_;
        bool target_detected_;

        // 传感器数据
        double current_center_x_;
        double current_center_y_;
        double current_center_z_;
        double avg_center_x_;
        double avg_center_y_;
        double avg_center_z_;

        // 采样相关
        std::vector<SamplePoint> sample_points_;
        rclcpp::Time sampling_start_time_;
        rclcpp::Time last_sample_time_;
        std::chrono::duration<double> sampling_duration_;

        // 运动控制
        double linear_x_;
        double linear_y_;
        double angular_z_;
        double prev_linear_x_;
        double prev_linear_y_;
        double prev_angular_z_;
        double target_linear_x_;
        double target_linear_y_;
        double movement_timer_;
        double movement_time_;

        // 速度平滑
        double max_linear_velocity_;
        double max_acceleration_;
        double max_angular_acceleration_;
        int controlling_frequency_;

        // 精细对准参数
        double position_tolerance_;      // 位置容忍度 (1cm)
        double fine_velocity_;           // 精细对准速度 (5mm/s)
        double dead_zone_threshold_;     // 死区阈值 (5mm)
        double sampling_timeout_;        // 采样超时 (5s)
        int max_retry_attempts_;         // 最大重试次数
        int retry_count_;

        // 时间戳
        rclcpp::Time reached_time_;

        
    };

} // namespace fine_alignment

#endif // FINE_ALIGNMENT_FINE_ALIGNER_HPP_